﻿using System;
using System.Text;
using System.Drawing;
using System.Buffers;
using System.Collections;
using System.Collections.Generic;
using System.Runtime.InteropServices;

public static partial class NativeAOT
{
    [UnmanagedCallersOnly(EntryPoint = "ngin")]
    public static unsafe int ngin(int m, int n, double eps1, double eps2, IntPtr x_ptr, IntPtr f_xa_ya_mn_ptr, IntPtr s_xa_ya_mn_ptr)
    {
        double* x = (double*)x_ptr.ToPointer();
        f_xa_ya_mn = Marshal.GetDelegateForFunctionPointer<delegatefunc_xa_ya_mn>(f_xa_ya_mn_ptr);
        s_xa_ya_mn = Marshal.GetDelegateForFunctionPointer<delegatefunc_xa_ya_mn>(s_xa_ya_mn_ptr);

        return ngin(m, n, eps1, eps2, x);
    }

    /// <summary>
    /// 非线性方程组最小二乘解
    /// f(int m,int n,double[] x, double[] p)计算非线性方程组各方程左端函数值的函数名。
    /// s(int m,int n,double[] x, double[] p)计算雅可比矩阵的函数名。
    /// </summary>
    /// <param name="m">非线性方程组中方程个数。</param>
    /// <param name="n">非线性方程组中未知数个数。m>=n。</param>
    /// <param name="eps1">控制最小二乘解的精度要求。</param>
    /// <param name="eps2">奇异值分解中的控制精度要求</param>
    /// <param name="x">x[n]存放初始近似值。返回最小二乘解，当m=n时即为非线性方程组的解。</param>
    /// <returns>函数返回迭代次数。本函数最大迭代次数为100。若迭代次数<0则表示奇异值分解失败。</returns>
    public static unsafe int ngin(int m, int n, double eps1, double eps2, double* x)
    {
        int i, j, k, l, kk, jt, interation, ka;
        double alpha, z = 0.0, h2, y1, y2, y3, y0, h1;
        double[] y = new double[10];
        double[] b = new double[10];

        double* p = stackalloc double[m * n];
        double* d = stackalloc double[m];
        double* pp = stackalloc double[n * m];
        double* dx = stackalloc double[n];
        double* u = stackalloc double[m * m];
        double* v = stackalloc double[n * n];
        double* w = stackalloc double[m + 1];

        //最大迭代次数
        interation = 100;
        ka = m + 1;
        l = 0; alpha = 1.0;
        while (l < interation)
        {
            //计算非线性方程组各方程左端函数值
            f_xa_ya_mn(x, d, m, n);
            //计算雅可比矩阵
            s_xa_ya_mn(x, p, m, n);
            //求广义逆
            jt = gmiv(p, m, n, d, dx, pp, eps2, u, v, ka, 100);
            if (jt < 0)
            {
                return (-1);
            }
            j = 0; jt = 1; h2 = 0.0;
            while (jt == 1)
            {
                jt = 0;
                if (j <= 2) z = alpha + 0.01 * j;
                else z = h2;
                for (i = 0; i <= n - 1; i++)
                {
                    w[i] = x[i] - z * dx[i];
                }
                // 计算非线性方程组各方程左端函数值
                f_xa_ya_mn(w, d, m, n);
                y1 = 0.0;
                for (i = 0; i <= m - 1; i++)
                {
                    y1 = y1 + d[i] * d[i];
                }
                for (i = 0; i <= n - 1; i++)
                {
                    w[i] = x[i] - (z + 0.00001) * dx[i];
                }
                // 计算非线性方程组各方程左端函数值
                f_xa_ya_mn(w, d, m, n);
                y2 = 0.0;
                for (i = 0; i <= m - 1; i++)
                {
                    y2 = y2 + d[i] * d[i];
                }
                y0 = (y2 - y1) / 0.00001;
                if (Math.Abs(y0) > 1.0e-10)
                {
                    h1 = y0;
                    h2 = z;
                    if (j == 0)
                    {
                        y[0] = h1;
                        b[0] = h2;
                    }
                    else
                    {
                        y[j] = h1; kk = 0; k = 0;
                        while ((kk == 0) && (k <= j - 1))
                        {
                            y3 = h2 - b[k];
                            if (Math.Abs(y3) + 1.0 == 1.0) kk = 1;
                            else h2 = (h1 - y[k]) / y3;
                            k = k + 1;
                        }
                        b[j] = h2;
                        if (kk != 0) b[j] = 1.0e+35;
                        h2 = 0.0;
                        for (k = j - 1; k >= 0; k--)
                        {
                            h2 = -y[k] / (b[k + 1] + h2);
                        }
                        h2 = h2 + b[0];
                    }
                    j = j + 1;
                    if (j <= 7) jt = 1;
                    else z = h2;
                }
            }
            alpha = z;
            y1 = 0.0;
            y2 = 0.0;
            for (i = 0; i <= n - 1; i++)
            {
                dx[i] = -alpha * dx[i];
                x[i] = x[i] + dx[i];
                y1 = y1 + Math.Abs(dx[i]);
                y2 = y2 + Math.Abs(x[i]);
            }
            if (y1 < eps1 * y2)
            {
                return (l);
            }
            l = l + 1;
        }
        return (l);
    }

    /*
      int main()
      { 
          int m,n,i;
          double eps1,eps2;
          void nginf(int,int,double [],double []);
          void ngins(int,int,double [],double []);
          double x[2]={0.5,-1.0};
          m=2; n=2;  eps1=0.000001; eps2=0.000001;
          i=ngin(m,n,eps1,eps2,x,nginf,ngins);
          if (i>0)
          {
              cout <<"迭代次数 = " <<i <<endl;
              for (i=0; i<=1; i++)  cout <<"x(" <<i <<") = " <<x[i] <<endl;
          }
          return 0;
      }
    // 计算非线性方程组各方程左端函数值
      void nginf(int m, int n, double x[], double d[])
      { 
          m=m; n=n;
          d[0]=x[0]*x[0]+10.0*x[0]*x[1]+4.0*x[1]*x[1]+0.7401006;
          d[1]=x[0]*x[0]-3.0*x[0]*x[1]+2.0*x[1]*x[1]-1.0201228;
        return;
      }
    // 计算雅可比矩阵
      void ngins(int m, int n, double x[], double p[])
      { 
          m=m;
             //p[0][0]
    p[0*n+0] = 2.0*x[0]+10.0*x[1];
             //p[0][1]
    p[0*n+1] = 10.0*x[0]+8.0*x[1];
             //p[1][0]
    p[1*n+0] = 2.0*x[0]-3.0*x[1];
             //p[1][1]
    p[1*n+1] = -3.0*x[0]+4.0*x[1];
          return;
      }
    */
    /*
    // 非线性方程组最小二乘解例2
      int main()
      { 
          int m,n,i;
          double eps1,eps2;
          void nginf(int,int,double [],double []);
          void ngins(int,int,double [],double []);
          double x[2]={1.0,-1.0};
          m=3; n=2; eps1=0.000001; eps2=0.000001;
          i = ngin(m,n,eps1,eps2,x,nginf,ngins);
          if (i>0)
          {
              cout <<"迭代次数 = " <<i <<endl;
              for (i=0; i<=1; i++)  cout <<"x(" <<i <<") = " <<x[i] <<endl;
          }
          return 0;
      }
    // 计算非线性方程组各方程左端函数值
      void nginf(int m, int n, double x[], double d[])
      { 
          m=m; n=n;
          d[0] = x[0]*x[0]+7.0*x[0]*x[1]+3.0*x[1]*x[1]+0.5;
          d[1] = x[0]*x[0]-2.0*x[0]*x[1]+x[1]*x[1]-1.0;
          d[2] = x[0]+x[1]+1.0;
          return;
      }
    // 计算雅可比矩阵
      void ngins(int m, int n, double x[], double p[])
      { 
          m=m;
             //p[0][0]
    p[0*n+0] = 2.0*x[0]+7.0*x[1];
             //p[0][1]
    p[0*n+1] = 7.0*x[0]+6.0*x[1];
             //p[1][0]
    p[1*n+0] = 2.0*x[0]-2.0*x[1];
             //p[1][1]
    p[1*n+1] = -2.0*x[0]+2.0*x[1];
             //p[2][0]
    p[2*n+0] = 1.0;
             //p[2][1]
    p[2*n+1] = 1.0;
          return;
      }
    */
}

